//
// Created by qzj on 2021/4/25.
//

#include "config.h"
#include "CommonFunc.h"

YAML::Node Config::config_node_ = YAML::Node();
std::string Config::lidar_topic;
std::string Config::imuTopic;
std::string Config::gnss_topic;
std::string Config::vel_topic;

std::string Config::map_dir_;
std::string Config::global_map_;
std::string Config::gnss_map_;
Eigen::Vector3d Config::gnss_origin_;

bool Config::use_gnss;
bool Config::use_wheel;
bool Config::use_ring;
bool Config::use_lidar_time;
float Config::wheel_scale_;

Eigen::Matrix4d Config::car_to_imu_;
Eigen::Matrix4d Config::imu_to_camera_;
Eigen::Matrix4d Config::camera_to_car_;
Eigen::Matrix4d Config::imu_to_car_;
Eigen::Matrix4d Config::camera_to_imu_;
Eigen::Matrix4d Config::car_to_camera_;
Eigen::Matrix4d Config::lego_to_lidar_;

std::string Config::init_method_;
float Config::prior_ori_;
float Config::car_height_;


template<typename T>
Eigen::Matrix<T, 4, 4> EigenIsoInv(const Eigen::Matrix<T, 4, 4> &Tcw) {
    Eigen::Matrix<T, 3, 3> Rcw = Tcw.block(0, 0, 3, 3);
    Eigen::Matrix<T, 3, 1> tcw = Tcw.block(0, 3, 3, 1);
    Eigen::Matrix<T, 3, 3> Rwc = Rcw.transpose();
    Eigen::Matrix<T, 3, 1> twc = -Rwc * tcw;

    Eigen::Matrix<T, 4, 4> Twc = Eigen::Matrix<T, 4, 4>::Identity();

    Twc.block(0, 0, 3, 3) = Rwc;
    Twc.block(0, 3, 3, 1) = twc;

    return Twc;
}

void Config::readConfig() {

    config_node_ = YAML::LoadFile(avp_slam::MATCH_YAML_PATH);

    lidar_topic = config_node_["lidar_topic"].as<std::string>();
    imuTopic = config_node_["imuTopic"].as<std::string>();
    gnss_topic = config_node_["gnss_topic"].as<std::string>();
    vel_topic = config_node_["vel_topic"].as<std::string>();

    map_dir_ = config_node_["map_dir"].as<std::string>();
    map_dir_ = avp_slam::WORK_SPACE_PATH + map_dir_;

    global_map_ = config_node_["global_map"].as<std::string>();
    global_map_ = map_dir_ + global_map_;

    gnss_map_ = config_node_["gnss_map"].as<std::string>();
    gnss_map_ = map_dir_ + gnss_map_;
    std::string gnss_origin_path = map_dir_ + config_node_["gnss_origin"].as<std::string>();
    std::ifstream fTimes;
    fTimes.open(gnss_origin_path.c_str());
    std::string s;
    getline(fTimes, s);
    std::istringstream is(s);
    is >> gnss_origin_(0) >> gnss_origin_(1) >> gnss_origin_(2);

    use_gnss = config_node_["use_gnss"].as<bool>();
    use_ring = config_node_["use_ring"].as<bool>();
    use_wheel = config_node_["use_wheel"].as<bool>();
    use_lidar_time = config_node_["use_lidar_time"].as<bool>();
    wheel_scale_ = config_node_["wheel_scale"].as<float>();

    for (size_t i = 0; i < 4; i++)
        for (size_t j = 0; j < 4; j++) {
            car_to_imu_(i, j) = config_node_["car_to_imu"][4 * i + j].as<float>();
        }
    car_to_imu_.block<3, 3>(0, 0) = car_to_imu_.block<3, 3>(0, 0) * wheel_scale_;

    for (size_t i = 0; i < 4; i++)
        for (size_t j = 0; j < 4; j++) {
            imu_to_camera_(i, j) = config_node_["imu_to_camera"][4 * i + j].as<float>();
        }

    for (size_t i = 0; i < 4; i++)
        for (size_t j = 0; j < 4; j++) {
            camera_to_car_(i, j) = config_node_["camera_to_car"][4 * i + j].as<float>();
        }

    for (size_t i = 0; i < 4; i++)
        for (size_t j = 0; j < 4; j++) {
            lego_to_lidar_(i, j) = config_node_["lego_to_lidar"][4 * i + j].as<float>();
        }

    imu_to_car_ = EigenIsoInv(car_to_imu_);
    camera_to_imu_ = EigenIsoInv(imu_to_camera_);
    car_to_camera_ = EigenIsoInv(camera_to_car_);

    init_method_ = config_node_["init_method"].as<std::string>();
    prior_ori_ = config_node_["prior_ori"].as<float>();
    car_height_ = config_node_["car_height"].as<float>();
}